#ifndef JACOBI_HPP
#define JACOBI_HPP

#include <iostream>
#include <vector>
#include "Eigen/Dense"

using namespace Eigen;
using namespace std;
/**********************Jacobi Iteration Mv = b********************/
class iteration
{
private:
    MatrixXd M;
    MatrixXd L, U, D, T;
    VectorXd v, b;
    double omega;

public:
    iteration() {}
    iteration(MatrixXd _A, VectorXd _b, double _w)
    {
        M = -_A;
        b = _b;
        omega = _w;
        U = M.triangularView<StrictlyUpper>();
        L = M.triangularView<StrictlyLower>();
        D = L + U - M;
        M = _A;
    }
    VectorXd weighted_jacobi_iteration(VectorXd _initial, int _times)
    {
        MatrixXd I = MatrixXd::Identity(M.rows(), M.cols());
        T = (1 - omega) * I + omega * D.inverse() * (L + U);
        v = _initial;
        for (int i = 0; i < _times; i++)
            v = T * v + omega * D.inverse() * b;
        return v;
    }
};

class Iteration
{
private:
    MatrixXd M, D;
    VectorXd v, b;
    double omega;

public:
    Iteration() {}
    Iteration(MatrixXd _A, VectorXd _b, double _w)
    {
        M = _A;
        b = _b;
        omega = _w;
        D = MatrixXd::Zero(M.rows(),M.cols());
        for (int i = 0; i < v.size(); i++)
            D(i, i) = 1.0 / M(i, i);
    }
    VectorXd weighted_jacobi_iteration(VectorXd _initial, int _times)
    {
        v = _initial;
        for (int i = 0; i < b.size(); i++)
            b(i) = b(i) * omega / M(i, i);
        for (int k = 0; k < _times; k++)
        {
            VectorXd vtmp = VectorXd::Zero(v.size());
            for (int i = 0; i < v.size(); i++)
            {
                for (int j = 0; j < v.size(); j++)
                {
                    if (j != i)
                        vtmp(i) -= (M(i, j) * omega / M(i, i)) * v(j);
                    else if (j == i)
                        vtmp(i) += (1 - omega) * v(j);
                }
                //vtmp(i) += omega * b(i) / M(i,i);
            }
            v = vtmp + b;
            
        }
        return v;
    }
};

#endif